#include <Servo.h>
//#include <armLib.h>
#include <Arm.h>

#define INLENGTH 17
#define INTERMINATOR 15
char inString[INLENGTH+1];
int inCount;

//arm bob;
Arm bob;

void setup()
{
  bob.initArm();
  
  //Start serial port
  Serial.begin(9600);
  Serial.println("Ready");
  digitalWrite(13, HIGH); //Power Light, going to use this as an error light also
  
  establishContact(); //Establishes Serial communication
}

void loop() 
{
  static int axis = 0;
  static int angle = 0;
  static int x_pos = 0;
  static int y_pos = 0;
  static int z_pos = 0;  
    
  readCommand();
  if( inString[0] == 'a' && inString[inCount - 1] == 's')
  {
     Serial.println( "Command Received" );
     inCount = 0;
     int angleCount = 2;
     axis = inString[ 1 ] - '0';
     Serial.println( inString[ 1 ]);
     do {
         angle = angle * 10 + inString[ angleCount ] - '0';
         angleCount++;
     }while (inString[ angleCount ] != 's');
     
     Serial.print( "Axis: " );
     Serial.print( axis );
     Serial.print( " Angle: ");
     Serial.println( angle );
     bob.setAxisAngle( axis, angle );
     angle = 0;
  }
  else if( inString[0] == 'r' && inString[inCount - 1] == 's')
  {
    Serial.println( "Command Received" );
    //bob.goToCal();
    bob.goHome();
  }
  else if( inString[0] == 'i' && inString[inCount - 1] == 's' )
  { 
    Serial.println( "Command Received" );
    int pos_Count = 2;
    
     do {
         x_pos = x_pos * 10 + inString[ pos_Count ] - '0';
         pos_Count++;
     }while (inString[ pos_Count ] != 'y');
     
     pos_Count++; 
     
     do {
         y_pos = y_pos * 10 + inString[ pos_Count ] - '0';
         pos_Count++;
     }while (inString[ pos_Count ] != 'z');
    
    pos_Count++;
    
    do {
         z_pos = z_pos * 10 + inString[ pos_Count ] - '0';
         pos_Count++;
     }while (inString[ pos_Count ] != 's');
     
     Serial.print( "x: " );
     Serial.print( x_pos );
     Serial.print( " y: " );
     Serial.print( y_pos );
     Serial.print( " z: ");
     Serial.println( z_pos );
    
     bob.setTCP( x_pos, y_pos, z_pos, 10 );
     x_pos = 0;
     y_pos = 0;
     z_pos = 0; 
  }
  
  //repeatCommand();
  
  delay(500);
  Serial.println("Ready");
  establishContact();
  
}

void repeatCommand()
{
  for(int i = 0; i < INTERMINATOR; i++)
    Serial.println( inString[ i ] );
}
  
void readCommand()
{
  inCount = 0;
  do {
        inString[inCount] = Serial.read();
        if (inString [inCount] == INTERMINATOR) break;
        (++inCount < INLENGTH);
  } while (Serial.available() > 0);
  inString[inCount] = 0;                     // null terminate the string
}

void establishContact() {
  while (Serial.available() <= 0) {
    delay(300);
  }
}

